#ifndef GET_STATE_H
#define GET_STATE_H

#include "cyberarm_controller/arm_state_msg.h"

#define MOTOR_NUM 4
cyberarm_controller::arm_state_msg current_state;
void GetState(const cyberarm_controller::arm_state_msg &msg)
{
    for(unsigned int i = 0; i < MOTOR_NUM; i++)
    {
        current_state.id[i] = msg.id[i];
        current_state.pos_estimate[i] = msg.pos_estimate[i];
        current_state.vel_estimate[i] = msg.vel_estimate[i];
        current_state.torque_estimate[i] = msg.torque_estimate[i];
        current_state.temp[i] = msg.temp[i];
    }
}

int GetInitState()
{
    while(current_state.id[0] != 1 || current_state.id[1] != 2 || current_state.id[2] != 3 || current_state.id[3] != 4)
    {
        ROS_ERROR("id = %d, %d, %d, %d", current_state.id[0], current_state.id[1], current_state.id[2], current_state.id[3]);
    }

    return 0;
}

std::vector<double> GetCurrentPosition()
{
    static ros::NodeHandle nh;
    static ros::Subscriber state_msg_pub = nh.subscribe("arm_state", 1000, GetState);
    static std::vector<double> current_pos(MOTOR_NUM);
    
    while(current_state.id[0] != 1 || current_state.id[1] != 2 || current_state.id[2] != 3 || current_state.id[3] != 4)
    {
    //     //wait
        ROS_ERROR("id = %d, %d, %d, %d", current_state.id[0], current_state.id[1], current_state.id[2], current_state.id[3]);
        ros::spinOnce();
    }

    for(unsigned int i = 0; i < MOTOR_NUM; i++)
    {
        current_pos[i] = current_state.pos_estimate[i];
    }
    return current_pos;
}

#endif // GET_STATE_H